#include <task_logic.h>
#include <dirent.h>
#include "yaml-cpp/yaml.h"
#include <SDL/SDL_image.h>
#define LOCALIZATION_WAIT_TIME 6		//wait n(s) to shutdown previous process before restart a new one
#define NAVIGATION_WAIT_TIME 6

#ifdef HAVE_YAMLCPP_GT_0_5_0
// The >> operator disappeared in yaml-cpp 0.5, so this function is
// added to provide support for code written under the yaml-cpp 0.3 API.
template<typename T>
void operator >> (const YAML::Node& node, T& i)
{
  i = node.as<T>();
}
#endif
stringstream smartgo_path_buf;
string yaml_uri = "/home/smartgo/config/common.yaml";

CloudApi::CloudApi(ros::NodeHandle &nh)
{
	config_file_path_ = nh.param<std::string>("config_file_path","/home/smartgo/config/");
	parking_flag_ = nh.param<bool>("parking",false);
	dis_to_park_ = nh.param<double>("dis_to_park",0.9);

	int speed_id = 1112;
	set_speed_shm.shm_init(speed_id,speed_id<<4);
	set_speed_shm.shm_write("2");

	int map_name_id = 1114;
	set_map_name_shm.shm_init(map_name_id,map_name_id<<4);
	set_map_name_shm.shm_write("map_nav");
	init_data();

	console_pub_ = nh.advertise<common_msg::Console>("/console_msg",2);
	cmd_pub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
	path_pub_ = nh.advertise<nav_msgs::Path>("/api_path", 1);
	parking_pub_ = nh.advertise<nav_msgs::Path>("/parking", 1);
	goal_op_pub_ = nh.advertise<std_msgs::String>("/goal_op", 1);
	run_status_pub_ = nh.advertise<common_msg::RunStatus>("/run_status", 1);
	scan_pub_ = nh.advertise<std_msgs::String>("/scan_op", 1);
	mapping_pub_ = nh.advertise<std_msgs::String>("/mapping_op", 1);
	initpos_pub_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose",1);
	goal_pub_ = nh.advertise<geometry_msgs::PoseStamped>("goal", 10);
	
	//sensors
	laser_3d_sub_ = nh.subscribe("/lidar_3d_point",1,&CloudApi::laser3DCallback,this);
	laser_2d_sub_ = nh.subscribe("/lidar_2d_point",1,&CloudApi::laser2DCallback,this);
	camera_sub_ = nh.subscribe("/camera_point_cloud",1,&CloudApi::cameraCallback,this);
	drive_sub_ = nh.subscribe("/drive",1,&CloudApi::driveCallback,this);
	odom_sub_ = nh.subscribe("/odom",1,&CloudApi::odomCallback,this);

	scan_sub_ = nh.subscribe("/scan_status",1,&CloudApi::scanStatusCallback,this);
	mapping_sub_ = nh.subscribe("/mapping_status",1,&CloudApi::mappingStatusCallback,this);
	process_sub_ = nh.subscribe("/kill_smartgo_process", 2, &CloudApi::killStatusCallback,this);
    navi_sub_ = nh.subscribe("/navi_status", 1, &CloudApi::naviStatusCallback, this);

	object_sub_ = nh.subscribe<common_msg::DetectedObjects>("/darknet_ros_node/detected_objects", 1, &CloudApi::objectCallback, this);

	current_map_ = nh.param<std::string>("map_name","map_nav");

	get_map_list();

	thread t1(&CloudApi::driver_thread,this);
	t1.detach();
	
	thread t2(&CloudApi::polling,this);
	t2.detach();

	MapParam map_param;
	map_param.mapName = "map_nav";
	map_param.createdAt = "today";
	string uri = config_file_path_+"map_nav/map_nav.yaml";
	char *fin = const_cast<char*>(uri.c_str());
#ifdef HAVE_YAMLCPP_GT_0_5_0
	// The document loading process changed in yaml-cpp 0.5.
	YAML::Node doc = YAML::LoadFile(fin);
#else
	YAML::Parser parser(fin);
	YAML::Node doc;
	parser.GetNextDocument(doc);
#endif
	try {
		doc["resolution"] >> map_param.mapInfo.resolution;
	} catch (YAML::InvalidScalar) {
		ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
		exit(-1);
	}
	try {
		doc["origin"][0] >> map_param.mapInfo.originX;
		doc["origin"][1] >> map_param.mapInfo.originY;
		//doc["origin"][2] >> 0;
	} catch (YAML::InvalidScalar) {
		ROS_ERROR("The map does not contain an origin tag or it is invalid.");
		exit(-1);
	}
	SDL_Surface* img;
	string pgm_uri = config_file_path_+"map_nav/map_nav.pgm";
	char *fname = const_cast<char*>(pgm_uri.c_str());
	if(!(img = IMG_Load(fname)))
	{
		std::string errmsg = std::string("failed to open image file \"") +
				std::string(fname) + std::string("\": ") + IMG_GetError();
		throw std::runtime_error(errmsg);
	}
	map_param.mapInfo.gridHeight = img->h;
	map_param.mapInfo.gridWidth = img->w;
	map_list_.push_back(map_param);
	current_map_ = map_param.mapName;
	load_map(current_map_);
	if(parking_flag_)
	{
		sleep(3);
		get_navi_point();
		get_navi_path();
		//add pre red and pre park
		{
			int size = navi_point_.size();
			if(size<2)	return;
			Point pre_red, pre_park;
			pre_red.mapName = pre_park.mapName = navi_path_[0].mapName;
			pre_red.pointName = "PR";
			pre_park.pointName = "PP";
			int parking[3];
			int CC[3];
			for(int i=0;i<navi_point_.size();i++)
			{
				if(navi_point_[i].pointName == "P")
				{
					parking[0] = navi_point_[i].gridX;
					parking[1] = navi_point_[i].gridY;
					parking[2] = navi_point_[i].angle;
				}
				if(navi_point_[i].pointName == "C")
				{
					CC[0] = navi_point_[i].gridX;
					CC[1] = navi_point_[i].gridY;
					CC[2] = navi_point_[i].angle;
				}
			}
			double angle = atan2(CC[1]-parking[1],CC[0]-parking[0]);
			int x = parking[0] + cos(angle)*dis_to_park_/resolution_;
			int y = parking[1] + sin(angle)*dis_to_park_/resolution_;
			pre_park.gridX = pre_red.gridX = x;
			pre_park.gridY = pre_red.gridY = y;
			pre_park.angle = parking[2] + 180;
			pre_park.angle = pre_park.angle < 360 ? pre_park.angle : pre_park.angle - 359;
			pre_red.angle = parking[2]+90;
			pre_red.angle = pre_red.angle<360 ? pre_red.angle : pre_red.angle - 359;
			navi_point_.insert(navi_point_.end(),pre_red);
			navi_point_.insert(navi_point_.end(),pre_park);

			navi_path_[0].point.insert(navi_path_[0].point.begin(),"PR");
			navi_path_[0].point.insert(navi_path_[0].point.end()-1,"PP");
		}
		ptree nav_path;
		nav_path.put<string>("mapName",navi_path_[0].mapName,string_translator<string>());
		nav_path.put<string>("pathName",navi_path_[0].pathName,string_translator<string>());


		write_json_fix(smartgo_path_buf, nav_path);

		navi_along_path(const_cast<char*>(smartgo_path_buf.str().c_str()));
	}
}

CloudApi::~CloudApi()
{
	common_msg::Console console_msg;
	console_msg.driver_stop = 1;
	console_msg.localization_stop = 1;
	console_msg.filter_stop = 1;
	console_msg.mapping_stop = 1;
	console_msg.navigation_stop = 1;
	console_msg.lidar_stop = 1;
	console_msg.loadmap_stop = 1;
	console_pub_.publish(console_msg);

	usleep(500000);
	if(ac_ != NULL)
		delete ac_;

	//set_speed_shm.remove_shm();
}
void CloudApi::init_data()
{
	origin_x_ = 0.0;
	origin_y_ = 0.0;
	resolution_ = 0.05;
	size_x_ = 1000;
	size_y_ = 1000;
	current_pos_.gridX = 0;
	current_pos_.gridY = 0;
	current_pos_.angle = 0;
	sonar_data_[0] = 0.56;
	infra_data_[1] = true;
	infra_data_[2] = false;
	infra_data_[3] = true;
	key_data_[0] = false;
	key_data_[1] = false;
	key_data_[2] = true;
	robot_status_ = IDLE;
	current_control_mode_ = MANUAL;
	current_map_ = "test_map";
}
void CloudApi::killStatusCallback(const std_msgs::String::ConstPtr& msg)
{
	if(msg->data == "kill_smartgo_process")
	{
		common_msg::Console console_msg;
		console_msg.driver_stop = 1;
		console_msg.localization_stop = 1;
		console_msg.filter_stop = 1;
		console_msg.mapping_stop = 1;
		console_msg.lidar_stop = 1;
		console_msg.loadmap_stop = 1;
		console_msg.navigation_stop = 1;
		console_pub_.publish(console_msg);
		sleep(1);
		console_pub_.publish(console_msg);

		ROS_INFO("shutdown cloud api");
		ros::shutdown();
	}
}
void CloudApi::polling()
{
//	ac_->waitForServer();
	ros::Rate r(1);
	common_msg::RunStatus run_status;
	while(ros::ok())
	{
		tf::Stamped<tf::Pose> global_pose;
		if(getRobotPose(global_pose))
		{
			run_status.header.stamp = ros::Time::now();
			unsigned int mx,my;
			double wx = global_pose.getOrigin().x();
			double wy = global_pose.getOrigin().y();
			ROS_DEBUG("origin_X:%f",wx);
			ROS_DEBUG("origin_Y:%f",wy);
			ROS_DEBUG("resolution:%f",resolution_);
			if(worldToMap(wx,wy,mx,my))
			{
				run_status.position.grid_x = mx;
				run_status.position.grid_y = my;
				int angle = angle_navi_to_api(tf::getYaw(global_pose.getRotation()));
				run_status.position.angle = angle;
				run_status.gps.altitude = 0.0;
				run_status.gps.latitude = 0.0;
				run_status.gps.longitude = 0.0;
				run_status.gps.status = 0;
				run_status_pub_.publish(run_status);
				current_pos_.gridX = mx;
				current_pos_.gridY = my;
				current_pos_.angle = angle;
			}
		}
		r.sleep();
	}
}

int CloudApi::angle_navi_to_api(double angle)
{
	angle = 57.3 * angle;
	while(angle<0)	
		angle = 360 + angle;
	return int(angle);
}

double CloudApi::angle_api_to_navi(int  angle)
{
	while(angle > 359)
		angle -= 360;
	while(angle < 0)
		angle += 360;

	if(angle >180)
		angle -= 360;
	double angle_rad = angle /57.3;
//	if(angle_rad > 3.1415)
//		angle_rad -= 3.1415;
	return angle_rad;
}
void CloudApi::scanStatusCallback(const std_msgs::String::ConstPtr& scan_status)
{
	scan_status_ = scan_status->data;
}
void CloudApi::mappingStatusCallback(const std_msgs::String::ConstPtr& mapping_status)
{
	string data = mapping_status->data;
	ROS_INFO("mapping status:%s",data.c_str());
	if(data == "finished")
		mapping_status_ = FINISHED_MAPPING;
	else if(data == "running")
		mapping_status_ = RUNNING_MAPPING;
	else
		mapping_status_ = FAILED_MAPPING;
}

void CloudApi::naviStatusCallback(const common_msg::NaviStatus::ConstPtr& navi_status)
{
	if(navi_status->navi_code == "N03")
	{
		if(current_num == total_point_num-1)
		{
			if(parking_flag_)
			{
				publish_parking_point();
				current_num++;
				//parking_flag_ = false;
				return;
			}
			else
			{
				ROS_INFO("get sub goal");
				nav_msgs::Path current_point;
				mutex_1.lock();
				current_point.header = point_queue.header;
				current_point.poses.push_back(point_queue.poses[current_num]);
				current_num++;
				mutex_1.unlock();
				path_pub_.publish(current_point);
			}
		}
		else if(current_num >= total_point_num)
		{
			ROS_INFO("task finished");
			robot_status_ = IDLE;
			if(parking_flag_)
			{
				ROS_INFO("loop run");				
				navi_along_path(const_cast<char*>(smartgo_path_buf.str().c_str()));
			}
		}
		else
		{	
			ROS_INFO("get sub goal");
			//if(current_num<total_point_num-1 && current_num > 1)
			//	sleep(5);
			nav_msgs::Path current_point;
			mutex_1.lock();
			current_point.header = point_queue.header;
			current_point.poses.push_back(point_queue.poses[current_num]);
			current_num++;
			mutex_1.unlock();
			path_pub_.publish(current_point);
		}
	}
}
void CloudApi::publish_parking_point()
{
	nav_msgs::Path current_point;
	std_msgs::Header header;
	header.stamp = ros::Time::now();
	header.frame_id = "map";
	current_point.header = header;
	int size = navi_path_.size();
	if(size == 0) return ;
	int point_size = navi_path_[0].point.size();
	if(point_size == 0) return ;

	geometry_msgs::PoseStamped pose_stamped;

	if(!get_navi_from_point(navi_path_[0].point[point_size-1], pose_stamped))
		return;
	pose_stamped.header = header;
	current_point.poses.push_back(pose_stamped);
	parking_pub_.publish(current_point);
}

bool CloudApi::getRobotPose(tf::Stamped<tf::Pose>& global_pose)
{
  string global_frame_ = "/map";
  string robot_base_frame_ = "/base_footprint";

  global_pose.setIdentity();
  tf::Stamped < tf::Pose > robot_pose;
  robot_pose.setIdentity();
  robot_pose.frame_id_ = robot_base_frame_;
  robot_pose.stamp_ = ros::Time();
  ros::Time current_time = ros::Time::now();  
  tf::TransformListener tf_;

  static double x_ = 0, y_ = 0, yaw_ = 0;
  try
  {
	tf_.waitForTransform(global_frame_,robot_base_frame_,ros::Time(0),ros::Duration(0.5));
    tf_.transformPose(global_frame_, robot_pose, global_pose);
	position_ok = true;
  }
  catch (...)
  {
    return false;
  }
  return true;
}
void CloudApi::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy)
{
  wx = origin_x_ + (mx + 0.5) * resolution_;
  wy = origin_y_ + (my + 0.5) * resolution_;
}
bool CloudApi::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my)
{
  if (wx < origin_x_ || wy < origin_y_)
    return false;

  mx = (int)((wx - origin_x_) / resolution_);
  my = (int)((wy - origin_y_) / resolution_);

  if (mx < size_x_ && my < size_y_)
    return true;

  return false;
}
ptree CloudApi::get_load_map_status()
{
	ptree pt;
	pt.put<string>("mapName",current_map_, string_translator<std::string>());
	pt.put<string>("status",map_load_status_, string_translator<std::string>());

    return pt;
}
bool CloudApi::edit_map(string mapName)
{
	if(is_map_exist(mapName))
	{
		current_map_ = mapName;
		return true;
	}
	else
	{
		return false;
	}
	
}
string CloudApi::load_map(string mapName)
{
	map_load_status_ = "loading";
	string pt;
	//params_map_name(mapName);
	set_map_name_shm.shm_write(mapName);
	//get_local_map();
	
	robot_status_ = IDLE;
	for(int i=0;i<map_list_.size();i++)
	{
		if(mapName == map_list_[i].mapName)
		{
			size_x_ = map_list_[i].mapInfo.gridWidth;
			size_y_ = map_list_[i].mapInfo.gridHeight;
			resolution_ = map_list_[i].mapInfo.resolution;
			origin_x_ = map_list_[i].mapInfo.originX;
			origin_y_ = map_list_[i].mapInfo.originY;
			break;
		}
	}

	position_ok = false;
	if(!map_loaded_)
	{
		map_loaded_ = true;
		current_map_ = mapName;
		std::thread t1(&CloudApi::localization_thread,this);
		t1.detach();
		std::thread t2(&CloudApi::navigation_thread,this);
		t2.detach();

	}
	else if(mapName != current_map_)
	{
		map_loaded_ = true;
		current_map_ = mapName;
		common_msg::Console console_msg;
		console_msg.localization_stop = 1;
		console_msg.navigation_stop = 1;
		console_msg.loadmap_stop = 1;
		console_pub_.publish(console_msg);
		sleep(1);
		console_pub_.publish(console_msg);
		sleep(LOCALIZATION_WAIT_TIME);
		std::thread t1(&CloudApi::localization_thread,this);
		t1.detach();
		std::thread t2(&CloudApi::navigation_thread,this);
		t2.detach();
	}
//	sleep(5);
	pt = get_map_list();
	get_navi_point();
	get_navi_path();
	get_task();
	map_load_status_ = "finished";
	return pt;
}
void CloudApi::set_navi_speed(int level)
{
	if(set_speed_shm.shm_write(std::to_string(level)))
	{
		usleep(1000);
		set_speed_shm.shm_write(std::to_string(level));
	}
}
void CloudApi::driver_thread()
{
	//system("/home/smartgo/package_ws/start_driver_laser");
}
void CloudApi::localization_thread()
{
	//system("/home/smartgo/package_ws/smartgo_localization");
}

void CloudApi::navigation_thread()
{
	//system("/home/smartgo/package_ws/smartgo_navigation");
}

void CloudApi::scan_thread()
{
	//system("/home/smartgo/package_ws/smartgo_scan");
}

void CloudApi::mapping_thread()
{
	//system("/home/smartgo/package_ws/smartgo_mapping");
}

bool CloudApi::params_map_name(string map_name)
{
	YAML::Node config = YAML::LoadFile(yaml_uri.c_str());

	ofstream fout;
	fout.open(yaml_uri.c_str(),ios::out);
	if(fout.is_open())
	{
		for(YAML::const_iterator it = config.begin();it != config.end(); ++it)
		{
			if(it->first.as<string>() == "map_name")
				fout<<(it->first.as<string>())<<": "<<map_name<<endl;
			else if(it->first.as<string>() == "speed_level")
				fout<<(it->first.as<string>())<<": "<< (it->second.as<int>())<<endl;
			else
				fout<<(it->first.as<string>())<<": "<< (it->second.as<double>())<<endl;
		}
		fout.close();
		return true;
	}
	fout.close();
	return false;
}

bool CloudApi::params_speed(int level)
{
	YAML::Node config = YAML::LoadFile(yaml_uri.c_str());

	ofstream fout;
	fout.open(yaml_uri.c_str(),ios::out);
	if(fout.is_open())
	{
		for(YAML::const_iterator it = config.begin();it != config.end(); ++it)
		{
			if(it->first.as<string>() == "speed_level")
				fout<<(it->first.as<string>())<<": "<<level<<endl;
			else if(it->first.as<string>() == "map_name")
				fout<<(it->first.as<string>())<<": "<< (it->second.as<string>())<<endl;
			else
				fout<<(it->first.as<string>())<<": "<< (it->second.as<double>())<<endl;
		}
		fout.close();
		return true;
	}
	fout.close();
	return false;
}
bool CloudApi::params_localization(string map_name,geometry_msgs::Pose pose)
{
	double init_pos[3],init_ori[4];
	init_pos[0] = pose.position.x;
	init_pos[1] = pose.position.y;
	init_pos[2] = pose.position.z;

	init_ori[0] = pose.orientation.w;
	init_ori[1] = pose.orientation.x;
	init_ori[2] = pose.orientation.y;
	init_ori[3] = pose.orientation.z;

	YAML::Node config = YAML::LoadFile(yaml_uri);
	ofstream fout;
	fout.open(yaml_uri.c_str(),ios::out);
	if(fout.is_open())
	{
		for(YAML::const_iterator it = config.begin();it != config.end(); ++it)
		{
			if(it->first.as<string>() == "speed_level")
				fout<<(it->first.as<string>())<<": "<<(it->second.as<string>())<<endl;
			else if(it->first.as<string>() == "map_name")
				fout<<(it->first.as<string>())<<": "<< map_name<<endl;
			else if(it->first.as<string>() == "init_pos_x")
				fout<<(it->first.as<string>())<<": "<< init_pos[0]<<endl;
			else if(it->first.as<string>() == "init_pos_y")
				fout<<(it->first.as<string>())<<": "<< init_pos[1]<<endl;
			else if(it->first.as<string>() == "init_pos_z")
				fout<<(it->first.as<string>())<<": "<< init_pos[2]<<endl;
			else if(it->first.as<string>() == "init_ori_w")
				fout<<(it->first.as<string>())<<": "<< init_ori[0]<<endl;
			else if(it->first.as<string>() == "init_ori_x")
				fout<<(it->first.as<string>())<<": "<< init_ori[1]<<endl;
			else if(it->first.as<string>() == "init_ori_y")
				fout<<(it->first.as<string>())<<": "<< init_ori[2]<<endl;
			else if(it->first.as<string>() == "init_ori_z")
				fout<<(it->first.as<string>())<<": "<< init_ori[3]<<endl;
		}
		fout.close();
		return true;
	}
	fout.close();
	return false;
}
void CloudApi::transform_point(string point_name,geometry_msgs::Pose &pose)
{
	geometry_msgs::PoseStamped pose_stamped;

	if(!get_navi_from_point(point_name, pose_stamped))
		return;
	pose = pose_stamped.pose;
}

void CloudApi::transform_point(int pos[2], int angle, geometry_msgs::Pose &pose)
{
	double wx,wy;
	mapToWorld(pos[0],pos[1],wx,wy);
	double angle_ = angle_api_to_navi(angle);
	geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(angle_);

	pose.position.x = wx;
	pose.position.y = wy;
	pose.position.z = 0;

	pose.orientation.w = quat.w;
	pose.orientation.x = quat.x;
	pose.orientation.y = quat.y;
	pose.orientation.z = quat.z;
}
void CloudApi::publish_init_position(geometry_msgs::Pose pose)
{
	geometry_msgs::PoseWithCovarianceStamped init_pose;
	init_pose.header.stamp = ros::Time::now();
	init_pose.pose.pose = pose;
	initpos_pub_.publish(init_pose);
}
void CloudApi::publish_goal(geometry_msgs::Pose goal)
{
	geometry_msgs::PoseStamped goal_pose;
	goal_pose.header.stamp = ros::Time::now();
	goal_pose.pose = goal;
	goal_pub_.publish(goal_pose);
}
void CloudApi::initialize_localizaition(string mapName, string point_name)
{
	if(current_map_ != mapName)
		return;

	current_init_point_ = point_name;

	geometry_msgs::Pose pose;
	transform_point(point_name,pose);

	publish_init_position(pose);

//	params_localization(mapName, pose);

	start_localization_navigation();
}

void CloudApi::initialize_localizaition(string mapName, double init_pos[3], double init_ori[4])
{
	if(current_map_ != mapName)
		return;
	
	geometry_msgs::Pose pose;
	pose.position.x = init_pos[0];
	pose.position.x = init_pos[1];
	pose.position.x = init_pos[2];

	pose.orientation.x = init_ori[0];
	pose.orientation.y = init_ori[1];
	pose.orientation.z = init_ori[2];
	pose.orientation.w = init_ori[3];

	publish_init_position(pose);
//	params_localization(mapName, pose);

	start_localization_navigation();
}

void CloudApi::initialize_localizaition(string mapName, int pos[2], int angle)
{
	if(current_map_ != mapName)
		return;

	geometry_msgs::Pose pose;
	transform_point(pos, angle, pose);

	publish_init_position(pose);

//	params_localization(mapName, pose);

	start_localization_navigation();
}
void CloudApi::initialize_localizaition()
{
	start_localization_navigation();
}
void CloudApi::start_localization_navigation()
{
/*	common_msg::Console console_msg;
	console_msg.localization_stop = 1;
	console_msg.navigation_stop = 1;
	console_msg.loadmap_stop = 1;
	console_pub_.publish(console_msg);
	ROS_INFO("start localization and navigation, waiting for task");
	sleep(5);

	std::thread t1(&CloudApi::localization_thread,this);
	t1.detach();
	sleep(1);
	std::thread t2(&CloudApi::navigation_thread,this);
	sleep(3);
	t2.detach();
*/
	ros::Rate r(1);
	while(ros::ok()&&!position_ok) //waited for valid position
	{
		r.sleep();
	}
	sleep(5);
}

ptree CloudApi::get_initialize_status()
{
	ptree pt,data;
	data.put<string>("currentMap",current_map_, string_translator<std::string>());
	data.put<string>("currentInitPoint",current_init_point_, string_translator<std::string>());
	pt.put_child("data",data);
	pt.put<string>("errorCode","", string_translator<std::string>());
	if(position_ok)
	{
		pt.put<string>("msg","initialized successfully", string_translator<std::string>());
		pt.put<bool>("successed",true);
	}
	else
	{
		pt.put<string>("msg","initializing", string_translator<std::string>());
		pt.put<bool>("successed",false);
	}
	return pt;
}

string CloudApi::get_navi_point()
{
	string uri = config_file_path_ + current_map_ +"/navi_point.json";
    
	ifstream in_(uri.c_str());
	ostringstream ss;
	char ch;
	while(ss&&in_.get(ch))
	ss.put(ch);
	in_.close();
	usleep(100000);
	ifstream in(uri.c_str());
	ptree point_info;
	try{
		read_json(in, point_info);
	}
	catch(...)
	{
		ROS_FATAL("navi point file do not exsit");
		in.close();
		return ss.str();
	}
	in.close();

    ptree temp;
	navi_point_.clear();
    for(ptree::iterator it=point_info.begin();it!=point_info.end();it++)
	{
		temp = it->second;
        Point tempPoint;
		tempPoint.mapName = temp.get<string>("mapName");
        tempPoint.pointName = temp.get<string>("pointName");
        tempPoint.gridX = temp.get<int>("gridX");;
        tempPoint.gridY = temp.get<int>("gridY");
        tempPoint.angle = temp.get<int>("angle");
        navi_point_.push_back(tempPoint);
    }
//	ROS_INFO("get navi point");
    return ss.str();
}

string CloudApi::get_file_string(string uri)
{
	ifstream in_(uri.c_str());	
	ostringstream ss;
	char ch;
	while(ss&&in_.get(ch))
	ss.put(ch);
	in_.close();
	return ss.str();
}

string CloudApi::get_init_point(string map_name)
{
	string uri = config_file_path_ + map_name +"/init_point.json";    
	return get_file_string(uri);
}

string CloudApi::get_calibration_point(string map_name)
{
	string uri = config_file_path_ + map_name +"/calibration_point.json";    
	return get_file_string(uri);
}
string CloudApi::get_navi_point(string map_name)
{
	string uri = config_file_path_ + map_name +"/navi_point.json";    
	return get_file_string(uri);
}
string CloudApi::get_work_point(string map_name)
{
	string uri = config_file_path_ + map_name +"/work_point.json";    
	return get_file_string(uri);
}
string CloudApi::get_charge_point(string map_name)
{
	string uri = config_file_path_ + map_name +"/charge_point.json";    
	return get_file_string(uri);
}
string CloudApi::get_user_point(string map_name)
{
	string uri = config_file_path_ + map_name +"/user_point.json";    
	return get_file_string(uri);
}
string CloudApi::get_navi_path(string map_name)
{
	string uri = config_file_path_ + map_name +"/navi_path.json";    
	return get_file_string(uri);
}
string CloudApi::get_virtual_wall(string map_name)
{
	string uri = config_file_path_ + map_name +"/virtual_wall.json";    
	return get_file_string(uri);
}
string CloudApi::get_virtual_wall_point(string map_name)
{
	string uri = config_file_path_ + map_name +"/virtual_wall_point.json";    
	return get_file_string(uri);
}

string CloudApi::get_user_path(string map_name)
{
	string uri = config_file_path_ + map_name +"/user_path.json";    
	return get_file_string(uri);
}

string CloudApi::get_user_path_point(string map_name)
{
	string uri = config_file_path_ + map_name +"/user_path_point.json";    
	return get_file_string(uri);
}

string CloudApi::get_slow_area(string map_name)
{
	string uri = config_file_path_ + map_name +"/slow_area.json";    
	return get_file_string(uri);
}
string CloudApi::get_stop_area(string map_name)
{
	string uri = config_file_path_ + map_name +"/stop_area.json";    
	return get_file_string(uri);
}
string CloudApi::get_user_area(string map_name)
{
	string uri = config_file_path_ + map_name +"/user_area.json";    
	return get_file_string(uri);
}
string CloudApi::get_slow_area_point(string map_name)
{
	string uri = config_file_path_ + map_name +"/slow_area_point.json";    
	return get_file_string(uri);
}
string CloudApi::get_stop_area_point(string map_name)
{
	string uri = config_file_path_ + map_name +"/stop_area_point.json";    
	return get_file_string(uri);
}
string CloudApi::get_user_area_point(string map_name)
{
	string uri = config_file_path_ + map_name +"/user_area_point.json";    
	return get_file_string(uri);
}

string CloudApi::get_navi_path()
{
	string uri = config_file_path_ + current_map_ +"/navi_path.json";
    navi_path_.clear();
	ifstream in_(uri.c_str());	
	ostringstream ss;
	char ch;
	while(ss&&in_.get(ch))
	ss.put(ch);
	in_.close();
	usleep(100000);
	ifstream in(uri.c_str());	
	ptree path_info;
	try{
		read_json(in, path_info);
	}
	catch(...)
	{
		ROS_FATAL("navi path file do not exsit");
		in.close();
		return ss.str();
	}
	in.close();

    ptree temp;
    for(ptree::iterator it=path_info.begin();it!=path_info.end();it++)
	{
		temp = it->second;
        Path tempPath;
		tempPath.mapName = temp.get<string>("mapName");
        tempPath.pathName = temp.get<string>("pathName");
        ptree point = temp.get_child("point");
        for(ptree::iterator it=point.begin();it!=point.end();it++)
        {
            tempPath.point.push_back(it->second.get_value<string>());
        }
        navi_path_.push_back(tempPath);
    }
//	ROS_INFO("get navi path");
	return ss.str();
}

string CloudApi::get_map_list()
{
	string uri = config_file_path_+"map.json";
    ifstream in_(uri.c_str());
	
	ostringstream ss;
	char ch;
	while(ss&&in_.get(ch))
	ss.put(ch);
	in_.close();

	usleep(100000);
	ifstream in(uri.c_str());
	ptree map_info;
	try{
		read_json(in, map_info);
	}
	catch(...)
	{
		ROS_FATAL("map file do not exsit");
		in.close();
		return ss.str();
	}
	in.close();

	ptree map_info_;
	try{
		map_info_ = map_info.get_child("map_list");
	}
	catch(...)
	{
		map_info_ = map_info;
	}

	ptree temp,temp_map_info;
	map_list_.clear();
    for(ptree::iterator it=map_info_.begin();it!=map_info_.end();it++)
	{
		temp = it->second;
        MapParam map_param;
		map_param.mapName = temp.get<string>("mapName");
        map_param.createdAt = temp.get<string>("createdAt");
		temp_map_info = temp.get_child("mapInfo");
        map_param.mapInfo.gridHeight = temp_map_info.get<int>("gridHeight");
		map_param.mapInfo.gridWidth = temp_map_info.get<int>("gridWidth");
		map_param.mapInfo.resolution = temp_map_info.get<double>("resolution");
		map_param.mapInfo.originX = temp_map_info.get<double>("originX");
		map_param.mapInfo.originY = temp_map_info.get<double>("originY");
        map_list_.push_back(map_param);
    }
//	ROS_INFO("map size:%d",map_list_.size());
	return ss.str();
}

bool CloudApi::is_map_exist(string map_name)
{
	for(int i=0;i<map_list_.size();i++)
	{
		if(map_name == map_list_[i].mapName)
			return true;
	}
	return false;
}
int CloudApi::rm_dir(std::string dir_full_path)
{    
    DIR* dirp = opendir(dir_full_path.c_str());    
    if(!dirp)
    {
        return -1;
    }
    struct dirent *dir;
    struct stat st;
    while((dir = readdir(dirp)) != NULL)
    {
        if(strcmp(dir->d_name,".") == 0
                || strcmp(dir->d_name,"..") == 0)
        {
            continue;
        }    
        std::string sub_path = dir_full_path + '/' + dir->d_name;
        if(lstat(sub_path.c_str(),&st) == -1)
        {
            continue;
        }    
        if(S_ISDIR(st.st_mode))
        {
            if(rm_dir(sub_path) == -1) // 如果是目录文件，递归删除
            {
                closedir(dirp);
                return -1;
            }
            rmdir(sub_path.c_str());
        }
        else if(S_ISREG(st.st_mode))
        {
            unlink(sub_path.c_str());     // 如果是普通文件，则unlink
        }
        else
        {
            continue;
        }
    }
    if(rmdir(dir_full_path.c_str()) == -1)//delete dir itself.
    {
        closedir(dirp);
        return -1;
    }
    closedir(dirp);
    return 0;
}
void CloudApi::remove_map(string map_name)
{
	string uri = config_file_path_+"map.json";
    ifstream in_(uri.c_str());
	
	usleep(100000);
	ifstream in(uri.c_str());
	ptree map_info;
	try{
		read_json(in, map_info);
	}
	catch(...)
	{
		ROS_FATAL("map file do not exsit");
	}
	in.close();

	ptree map_info_;
	try{
		map_info_ = map_info.get_child("map_list");
	}
	catch(...)
	{
		map_info_ = map_info;
	}

	ptree temp,temp_map_info,update_map,map_list_info;
	map_list_.clear();
	int map_num = 0;
    for(ptree::iterator it=map_info_.begin();it!=map_info_.end();it++)
	{
		temp = it->second;
        MapParam map_param;
		map_param.mapName = temp.get<string>("mapName");
		if(map_name == map_param.mapName)
			continue ;
        map_param.createdAt = temp.get<string>("createdAt");
		temp_map_info = temp.get_child("mapInfo");
        map_param.mapInfo.gridHeight = temp_map_info.get<int>("gridHeight");
		map_param.mapInfo.gridWidth = temp_map_info.get<int>("gridWidth");
		map_param.mapInfo.resolution = temp_map_info.get<double>("resolution");
		map_param.mapInfo.originX = temp_map_info.get<double>("originX");
		map_param.mapInfo.originY = temp_map_info.get<double>("originY");
        map_list_.push_back(map_param);

		ptree temp_pt;
		temp_pt.put<string>("mapName",map_param.mapName, string_translator<std::string>());
		temp_pt.put<string>("createdAt",map_param.createdAt, string_translator<std::string>());
		ptree temp_info;
		temp_info.put<int>("gridHeight",map_param.mapInfo.gridHeight);
		temp_info.put<int>("gridWidth",map_param.mapInfo.gridWidth);
		temp_info.put<double>("originX",map_param.mapInfo.originX);
		temp_info.put<double>("originY",map_param.mapInfo.originY);
		temp_info.put<double>("resolution",map_param.mapInfo.resolution);
		temp_pt.put_child("mapInfo",temp_info);
		map_list_info.push_back(make_pair("",temp_pt));
		map_num++;
    }
	if(map_num == 0)
	{
		ptree pt;
		map_list_info.push_back(make_pair("",pt));
	}
	update_map.put_child("map_list",map_list_info);
	write_json_fix(uri.c_str(),update_map);
}

bool CloudApi::del_map(string map_name)
{
	if(!is_map_exist(map_name))
	{
		return false;
	}

	string dir = config_file_path_ + map_name;
	if(access(dir.c_str(),0) == 0)
	{
		if(rm_dir(dir) == -1)
			return false;
		else
		{
			remove_map(map_name);
			get_map_list();
			return true;
		}
	}
	return false;
}

void CloudApi::update_navi_info()
{
	get_navi_point();
	get_navi_path();
}

string CloudApi::get_task()
{
	string uri = config_file_path_ + current_map_ +"/task.json";
    
	ifstream in_(uri.c_str());
	ostringstream ss;
	char ch;
	while(ss&&in_.get(ch))
	ss.put(ch);
	in_.close();
	usleep(100000);
	ifstream in(uri.c_str());
	ptree task_data,task_info;
	try{
		read_json(in, task_info);
	}
	catch(...)
	{
		//ROS_FATAL("task file do not exsit");
		return ss.str();
	}
	in.close();

    string mapName = task_info.get<string>("mapName");
	ptree task,temp,default_task;
	task = task_info.get_child("task");
	task_.clear();
	for(ptree::iterator it=task.begin();it!=task.end();it++)
	{
        temp = it->second;
        Task tempTask;

        tempTask.name = temp.get<string>("name");
        tempTask.mode = temp.get<string>("mode");
        tempTask.param = temp.get<string>("param");
        tempTask.start = temp.get<string>("start");
        tempTask.end = temp.get<string>("end");
        tempTask.run = temp.get<bool>("run");
        ptree repeat = temp.get_child("repeat");

        for(ptree::iterator it=repeat.begin();it!=repeat.end();it++)
        {
            tempTask.repeat.push_back(it->second.get_value<int>());
        }
        task_.push_back(tempTask);
	}

	default_task = task_info.get_child("defaultTask");
	default_task_.name = default_task.get<string>("name","");
	default_task_.mode = default_task.get<string>("mode","");
	default_task_.param = default_task.get<string>("param","");
//	ROS_INFO("get task");
	return ss.str();
}

void CloudApi::start_task(char buf[])
{
	stringstream ss(buf);
    ptree pt;
    read_json(ss,pt);
    current_task_.mapName = pt.get<string>("mapName");
    current_task_.name = pt.get<string>("name");
	current_task_.mode = pt.get<string>("mode");
	current_task_.param = pt.get<string>("param");
	current_task_.loopCount = pt.get<int>("loopCount");

	ptree task;
	task.put<string>("mapName",current_task_.mapName, string_translator<std::string>());
	if(current_task_.mode == "sentry")
		task.put<string>("pointName",current_task_.param, string_translator<std::string>());
	else
		task.put<string>("pathName",current_task_.param, string_translator<std::string>());

	ostringstream sss;
    write_json_fix(sss, task);
    string task_buf = sss.str();
	char *buf2 = const_cast<char*>(task_buf.c_str());
	
	if(current_task_.mode == "sentry")
	{
		navi_to_point(buf2);
	}
	else
	{
		navi_along_path(buf2);
	}
	robot_status_ = RUNNING;
}

ptree CloudApi::get_task_status()
{
	ptree task_status,data;
	if(robot_status_ == RUNNING)
	{
		data.put<string>("mapName",current_task_.mapName, string_translator<std::string>());
		data.put<string>("name",current_task_.name, string_translator<std::string>());
		data.put<string>("mode",current_task_.mode, string_translator<std::string>());
		data.put<string>("param",current_task_.param, string_translator<std::string>());
		data.put<int>("loopCount",current_task_.loopCount);
		task_status.put_child("data",data);
	}
	else
	{
		task_status.put<string>("data","", string_translator<std::string>());
	}
	
	task_status.put<string>("errorCode","", string_translator<std::string>());
	task_status.put<string>("msg","successed", string_translator<std::string>());
	task_status.put<bool>("successed",true);
	return task_status;
}

void CloudApi::set_move_cmd(char buf[])
{
	if(current_control_mode_ == AUTO)
		return ;

	stringstream ss(buf);
    ptree pt,pt_speed;
    read_json(ss,pt);
	pt_speed = pt.get_child("speed");
    double linearSpeed = pt_speed.get<double>("linearSpeed");
    double angularSpeed = pt_speed.get<double>("angularSpeed");
	ROS_INFO("linearSpeed:%f",linearSpeed);

	geometry_msgs::Twist vel;
	vel.linear.x = linearSpeed;
	vel.linear.y = 0;
	vel.linear.z = 0;
	vel.angular.x = 0;
	vel.angular.y = 0;
	vel.angular.z = angularSpeed;
	cmd_pub_.publish(vel);

	std_msgs::String goal_op;
	goal_op.data = "stop";
	goal_op_pub_.publish(goal_op);
	robot_status_ = IDLE;
}

void CloudApi::navi_to_point(char buf[])
{
	stringstream ss(buf);
    ptree pt;
    read_json(ss,pt);
    string mapName = pt.get<string>("mapName");
	string pointName = pt.get<string>("pointName");
	ROS_INFO("pointName:%s",pointName.c_str());

	nav_msgs::Path path;
	path.header.frame_id = "map";
	path.header.stamp = ros::Time::now();
	geometry_msgs::PoseStamped pose;
	pose.header.stamp = path.header.stamp;
	if(!get_navi_from_point(pointName, pose))
		return;

	path.poses.push_back(pose);
	path_pub_.publish(path);
}

void CloudApi::navi_to_dest(char buf[])
{
	stringstream ss(buf);
    ptree pt;
    read_json(ss,pt);
	Destination dest;
    string mapName = pt.get<string>("mapName");
	dest.gridX = pt.get<int>("gridX");
	dest.gridY = pt.get<int>("gridY");
	dest.angle = pt.get<int>("angle");

	nav_msgs::Path path;
	path.header.frame_id = "map";
	path.header.stamp = ros::Time::now();
	geometry_msgs::PoseStamped pose;

	pose.header.stamp = path.header.stamp;
	double wx,wy;
	if(dest.gridX<0||dest.gridY<0
		||dest.gridX>=size_x_||dest.gridY>=size_y_)
	{
		ROS_WARN("invalid dest");
		return ;
	}
	mapToWorld(dest.gridX,dest.gridY,wx,wy);
	pose.pose.position.x = wx;
	pose.pose.position.y = wy;
	double angle_ = angle_api_to_navi(dest.angle);
	geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(angle_);
	pose.pose.orientation = quat;
	path.poses.push_back(pose);

	path_pub_.publish(path);
	ROS_INFO("dest = (%d,%d)",dest.gridX,dest.gridY);
}

void CloudApi::navi_along_path(char buf[])
{
	robot_status_ = RUNNING;
	stringstream ss(buf);
    ptree pt;
    read_json(ss,pt);
    string mapName = pt.get<string>("mapName");
	string pathName = pt.get<string>("pathName");
	nav_msgs::Path path;
	path.header.frame_id = "map";
	path.header.stamp = ros::Time::now();

	for(int i=0;i<navi_path_.size();i++)
	{
		if(navi_path_[i].pathName == pathName) //find path
		{
			for(int j=0;j<navi_path_[i].point.size();j++)	//points in path
			{
				for(int k=0;k<navi_point_.size();k++)		//find correspond point in navi_point
				{
					if(navi_path_[i].point[j] == navi_point_[k].pointName)
					{
						geometry_msgs::PoseStamped pose;
						pose.header.stamp = path.header.stamp;
						
						double wx,wy;
						if(navi_point_[k].gridX<0||navi_point_[k].gridY<0
							||navi_point_[k].gridX>=size_x_||navi_point_[k].gridY>=size_y_)
						{
							ROS_WARN("invalid path point");
							return;
						}
						ROS_INFO("point:(%d,%d,%d)",navi_point_[k].gridX,navi_point_[k].gridY,navi_point_[k].angle);
						mapToWorld(navi_point_[k].gridX,navi_point_[k].gridY,wx,wy);
						pose.pose.position.x = wx;
						pose.pose.position.y = wy;
						double angle_ = angle_api_to_navi(navi_point_[k].angle);
						geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(angle_);
						pose.pose.orientation = quat;
						path.poses.push_back(pose);
					}
				}
			}
			break;
		}
	}
	nav_msgs::Path current_point;
	current_point.header = path.header;
	if(path.poses.size()>0)
	{
		mutex_1.lock();
		point_queue = path;
		current_num = 0;
		total_point_num = path.poses.size();
		current_point.poses.push_back(path.poses[current_num]);
		current_num++;
		mutex_1.unlock();

		path_pub_.publish(current_point);
	}
}

bool CloudApi::get_navi_from_point(string pointName,geometry_msgs::PoseStamped &pose)
{
	for(int i=0;i<navi_point_.size();i++)
	{
		if(navi_point_[i].pointName == pointName)
		{
			double wx,wy;
			if(navi_point_[i].gridX<0||navi_point_[i].gridY<0
				||navi_point_[i].gridX>=size_x_||navi_point_[i].gridY>=size_y_)
			{
				ROS_WARN("invalid goal point");
				return false;
			}

			mapToWorld(navi_point_[i].gridX,navi_point_[i].gridY,wx,wy);
			pose.pose.position.x = wx;
			pose.pose.position.y = wy;
			double angle_ = angle_api_to_navi(navi_point_[i].angle);
			geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(angle_);
			pose.pose.orientation = quat;
			return true;
		}
	}
	return false;
}

ptree CloudApi::get_basic_info()
{
	ptree basic_info;
	basic_info.put<int>("battery", 25);
	basic_info.put<bool>("charge", false);
	basic_info.put<int>("status", 1);
	return basic_info;
}

void CloudApi::set_control_mode(string mode)
{
	if(mode == "auto")
	{
		current_control_mode_ = AUTO;
	}
	else
	{
		current_control_mode_ = MANUAL;
	}
}

//sensor msg
void CloudApi::laser3DCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
	Point3D temp;
	sensor_msgs::PointCloud out_pointcloud;
	sensor_msgs::convertPointCloud2ToPointCloud(*msg,out_pointcloud);
	if(out_pointcloud.points.size()<5)
		return ;
	mutex_3d_lidar.lock();
	lidar_3d_data.clear();

	for(int i=1;i<out_pointcloud.points.size()-1;i+=15)
	{
		int x = out_pointcloud.points[i].x/resolution_;
		int y = out_pointcloud.points[i].y/resolution_;
		temp.gridZ =out_pointcloud.points[i].z/resolution_;
		double angle = angle_api_to_navi(current_pos_.angle);
		temp.gridX = x*cos(angle) - y*sin(angle)+current_pos_.gridX;
		temp.gridY = x*sin(angle) + y*cos(angle)+current_pos_.gridY;
		lidar_3d_data.push_back(temp);
	}
	mutex_3d_lidar.unlock();
}
void CloudApi::laser2DCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
	Point3D temp;
	sensor_msgs::PointCloud out_pointcloud;
	sensor_msgs::convertPointCloud2ToPointCloud(*msg,out_pointcloud);
	if(out_pointcloud.points.size()<5)
		return ;
	mutex_2d_lidar.lock();
	lidar_2d_data.clear();
	for(int i=1;i<out_pointcloud.points.size()-1;i+=5)
	{
		temp.gridX =out_pointcloud.points[i].x/resolution_;
		temp.gridY =out_pointcloud.points[i].y/resolution_;
		lidar_2d_data.push_back(temp);
	}
	mutex_2d_lidar.unlock();
}
void CloudApi::cameraCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
	Point3D temp;
	sensor_msgs::PointCloud out_pointcloud;
	sensor_msgs::convertPointCloud2ToPointCloud(*msg,out_pointcloud);
	if(out_pointcloud.points.size()<5)
		return ;
	mutex_camera.lock();
	camera_data.clear();
	for(int i=1;i<out_pointcloud.points.size()-1;i+=5)
	{
		temp.gridX =out_pointcloud.points[i].x/resolution_;
		temp.gridY =out_pointcloud.points[i].y/resolution_;
		temp.gridZ =out_pointcloud.points[i].z/resolution_;
		camera_data.push_back(temp);
	}
	mutex_camera.unlock();
}
void CloudApi::driveCallback(const common_msg::DriveInfo::ConstPtr& msg)
{
	sonar_data_[0] = msg->sonar0;
	sonar_data_[1] = msg->sonar1;
	sonar_data_[2] = 0.532;
	sonar_data_[3] = msg->sonar3;
	sonar_data_[4] = msg->sonar4;

	current_gps_.altitude = msg->altitude;
	current_gps_.latitude = msg->latitude;
	current_gps_.longitude = msg->longitude;
	current_gps_.status	= msg->gnss_flag;

	roll_ = 0.1;
	pitch_ = 0.11;
	yaw_ = 0.58;
}
void CloudApi::odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
	current_speed_.linearSpeed = msg->twist.twist.linear.x;
	current_speed_.angularSpeed = msg->twist.twist.angular.x;
}

bool CloudApi::download_file(char buf[])
{
	return true;
}

ptree CloudApi::get_3d_lidar()
{
	ptree lidar_3d;
	ptree robot_pose;
	robot_pose.put<int>("x",current_pos_.gridX);
	robot_pose.put<int>("y",current_pos_.gridY);
	robot_pose.put<int>("angle",current_pos_.angle);
	ptree data;
	mutex_3d_lidar.lock();
	for(int i=0;i<lidar_3d_data.size();i++)
	{
		ptree temp;
		temp.put<int>("x",lidar_3d_data[i].gridX);
		temp.put<int>("y",lidar_3d_data[i].gridY);
		temp.put<int>("z",lidar_3d_data[i].gridZ);
		data.push_back(std::make_pair("",temp));
	}
	mutex_3d_lidar.unlock();
	lidar_3d.put_child("robotPos",robot_pose);
	lidar_3d.put_child("data",data);
	return lidar_3d;
}
ptree CloudApi::get_2d_lidar()
{
	ptree lidar_2d;
	ptree robot_pose;
	robot_pose.put<int>("x",current_pos_.gridX);
	robot_pose.put<int>("y",current_pos_.gridY);
	robot_pose.put<int>("angle",current_pos_.angle);
	ptree data;
	mutex_2d_lidar.lock();
	for(int i=0;i<lidar_2d_data.size();i++)
	{
		ptree temp;
		temp.put<int>("x",lidar_2d_data[i].gridX);
		temp.put<int>("y",lidar_2d_data[i].gridY);
		data.push_back(std::make_pair("",temp));
	}
	mutex_2d_lidar.unlock();
	lidar_2d.put_child("robotPos",robot_pose);
	lidar_2d.put_child("data",data);
	return lidar_2d;
}
ptree CloudApi::get_camera()
{
	ptree camera;
	ptree robot_pose;
	robot_pose.put<int>("x",current_pos_.gridX);
	robot_pose.put<int>("y",current_pos_.gridY);
	robot_pose.put<int>("angle",current_pos_.angle);
	ptree data;
	mutex_camera.lock();
	for(int i=0;i<camera_data.size();i++)
	{
		ptree temp;
		temp.put<int>("x",camera_data[i].gridX);
		temp.put<int>("y",camera_data[i].gridY);
		temp.put<int>("z",camera_data[i].gridZ);
		data.push_back(std::make_pair("",temp));
	}
	mutex_camera.unlock();
	camera.put_child("robotPos",robot_pose);
	camera.put_child("data",data);
	return camera;
}

ptree CloudApi::get_sonar()
{
	ptree sonar;

	for(int i=0;i<5;i++)
		sonar.put<double>("sonar"+std::to_string(i),sonar_data_[i]);
	return sonar;
}
ptree CloudApi::get_infra()
{
	ptree infra;
	for(int i=0;i<3;i++)
		infra.put<bool>("infra"+std::to_string(i),infra_data_[i]);
	return infra;
}
ptree CloudApi::get_key()
{
	ptree key;
	for(int i=0;i<3;i++)
		key.put<bool>("key"+std::to_string(i),key_data_[i]);
	return key;
}
ptree CloudApi::get_gps()
{
	ptree gps;
	gps.put<double>("altitude",16.2);
	gps.put<double>("latitude",31.265481);
	gps.put<double>("longitude",253.154);
	gps.put<int>("status",0);
	return gps;
}
ptree CloudApi::get_odom()
{
	ptree odom;
	ptree linearSpeed,angularSpeed;
	linearSpeed.put<double>("x",current_speed_.linearSpeed);
	linearSpeed.put<double>("y",0);
	linearSpeed.put<double>("z",0);
	angularSpeed.put<double>("x",0);
	angularSpeed.put<double>("y",0);
	angularSpeed.put<double>("z",current_speed_.angularSpeed);
	odom.put_child("linearSpeed",linearSpeed);
	odom.put_child("angularSpeed",angularSpeed);
	return odom;
}
ptree CloudApi::get_imu()
{
	ptree imu;
	imu.put<double>("roll",roll_);
	imu.put<double>("pitch",pitch_);
	imu.put<double>("yaw",yaw_);
	return imu;
}
ptree CloudApi::get_rfid()
{
	ptree rfid;
	rfid.put<int>("id",251);
	rfid.put<int>("status",1);
	return rfid;
}
ptree CloudApi::get_uwb()
{
	ptree uwb;
	uwb.put<double>("x",0.25);
	uwb.put<double>("y",0.56);
	uwb.put<double>("z",0.245);
	uwb.put<int>("status",1);
	return uwb; 
}

void CloudApi::set_safe_area(double r1,double r2, double r3)
{
	safe_area_[0] = r1;
	safe_area_[1] = r2;
	safe_area_[2] = r3;
}
bool CloudApi::write_file(char buf[], string file_name)
{
	string file_path = config_file_path_ + current_map_+"/"+file_name;
	FILE *fp = fopen(file_path.c_str(),"w");
	if(fp!=NULL)
	{
		fwrite(buf,strlen(buf),1,fp);
		fclose(fp);
		return true;
	}
	fclose(fp);
	return false;
}

bool CloudApi::set_init_point(char buf[])
{
	string file_name = "init_point.json";
	return write_file(buf,file_name);
}

bool CloudApi::set_calibration_point(char buf[])
{
	string file_name = "calibration_point.json";
	return write_file(buf,file_name);
}

bool CloudApi::set_work_point(char buf[])
{
	string file_name = "work_point.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_charge_point(char buf[])
{
	string file_name = "charge_point.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_navi_point(char buf[])
{
	string file_name = "navi_point.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_user_point(char buf[])
{
	string file_name = "user_point.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_navi_path(char buf[])
{
	string file_name = "navi_path.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_virtual_wall(char buf[])
{
	string file_name = "virtual_wall.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_virtual_wall_point(char buf[])
{
	string file_name = "virtual_wall_point.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_user_path(char buf[])
{
	string file_name = "user_path.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_user_path_point(char buf[])
{
	string file_name = "user_path_point.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_slow_area(char buf[])
{
	string file_name = "slow_area.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_slow_area_point(char buf[])
{
	string file_name = "slow_area_point.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_stop_area(char buf[])
{
	string file_name = "stop_area.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_stop_area_point(char buf[])
{
	string file_name = "stop_area_point.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_user_area(char buf[])
{
	string file_name = "user_area.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_user_area_point(char buf[])
{
	string file_name = "user_area_point.json";
	return write_file(buf,file_name);
}
bool CloudApi::set_mapping_mode(string map_name, string mode)
{
	set_map_name_shm.shm_write(map_name);
	if(is_map_exist(map_name))
		;//return false;
	else
	{
		string dir = config_file_path_ + map_name;
		if(access(dir.c_str(),0) == -1)
		{
			int flag = mkdir(dir.c_str(),0777);
			if(flag != 0)
			{
				ROS_INFO("make file folder error!!!");
				return false;
			}
		}		
	}
	
	if(mode == "fast")
	{
		mapping_mode_ = FAST;
		ROS_INFO("fast mapping");
	}
	else
	{
		mapping_mode_ = PRECISE;
		ROS_INFO("precise mapping");
	}

	common_msg::Console console_msg;
	console_msg.localization_stop = 1;
//	console_msg.filter_stop = 1;
	console_msg.mapping_stop = 1;
//	console_msg.lidar_stop = 1;
	console_msg.loadmap_stop = 1;
	console_msg.navigation_stop = 1;
	console_pub_.publish(console_msg);
	usleep(50000);
	console_pub_.publish(console_msg);
	//sleep(5);

	return true;//params_map_name(map_name);
}
void CloudApi::start_scan()
{
	scan_status_ = "running";
	ROS_INFO("start scan");
	common_msg::Console console_msg;
	console_msg.localization_stop = 1;
//	console_msg.filter_stop = 1;
	console_msg.mapping_stop = 1;
//	console_msg.lidar_stop = 1;
	console_msg.loadmap_stop = 1;
	console_msg.navigation_stop = 1;
	console_pub_.publish(console_msg);
	usleep(50000);
	console_pub_.publish(console_msg);
	sleep(5);
	if(mapping_mode_ == FAST)
	{
		thread t1(&CloudApi::scan_thread,this);
		t1.detach();
	}
}
void CloudApi::cancel_scan()
{
	ROS_INFO("cancel scan");
	std_msgs::String str;
	str.data = "cancel_scan";
	scan_pub_.publish(str);
}
void CloudApi::finish_scan()
{
	ROS_INFO("finish scan");
	if(mapping_mode_ == FAST)
	{
		common_msg::Console console_msg;
		console_msg.mapping_stop = 1;
		console_pub_.publish(console_msg);
		usleep(50000);
		console_pub_.publish(console_msg);
	}
	
}
ptree CloudApi::get_scan_status()
{
	ptree pt;
	pt.put<string>("mapName",current_map_, string_translator<std::string>());
	pt.put<string>("status",scan_status_, string_translator<std::string>());
	return pt;
}
void CloudApi::start_mapping()
{
	ROS_INFO("start mapping");
	mapping_status_ = RUNNING_MAPPING;
	if(mapping_mode_ == FAST)
	{
		thread t1(&CloudApi::mapping_thread,this);
		t1.detach();
	}
}
void CloudApi::cancel_mapping()
{
	ROS_INFO("cancel mapping");
		std_msgs::String str;
	str.data = "cancel_mapping";
	mapping_pub_.publish(str);
}

void CloudApi::get_mapping_status(string &msg)
{
	switch(mapping_status_)
	{
		case FINISHED_MAPPING:
			msg = "FINISHED";
			break;
		case RUNNING_MAPPING:
			msg = "RUNNING";
			break;
		case FAILED_MAPPING:
			msg = "FAILED";
			break;
		default:
			msg = "FAILED";
	}
}

void CloudApi::global_position()
{
	initialize_localizaition();
}

void CloudApi::initial_position(char buf[])
{
	stringstream ss(buf);
    ptree pt,data;
    read_json(ss,pt);
	data = pt.get_child("initPoint");
	int pos[2],angle;
    pos[0] = data.get<int>("gridX");
	pos[1] = data.get<int>("gridY");
	angle = data.get<int>("angle");

	initialize_localizaition(current_map_,pos,angle);
}

void CloudApi::start_route(char buf[])
{
	stringstream ss(buf);
    ptree pt,temp;
    read_json(ss,pt);
	nav_msgs::Path path;
    for(ptree::iterator it=pt.begin();it!=pt.end();it++)
	{
		temp = it->second;
        int gridX = temp.get<int>("gridX");;
        int gridY = temp.get<int>("gridY");
        int angle = temp.get<int>("angle");

		geometry_msgs::PoseStamped pose;
		pose.header.stamp = path.header.stamp;		
		double wx,wy;
		mapToWorld(gridX,gridY,wx,wy);
		pose.pose.position.x = wx;
		pose.pose.position.y = wy;
		double angle_ = angle_api_to_navi(angle);
		geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(angle_);
		pose.pose.orientation = quat;
		path.poses.push_back(pose);
    }

	nav_msgs::Path current_point;
	if(path.poses.size()>0)
	{
		mutex_1.lock();
		point_queue = path;
		current_num = 0;
		total_point_num = path.poses.size();
		current_point.poses.push_back(path.poses[current_num]);
		current_num++;
		mutex_1.unlock();

		path_pub_.publish(current_point);
	}
}

ptree CloudApi::get_running_status()
{
	ptree info;
	string status;
	switch(robot_status_)
	{
		case RUNNING:
			status = "running";
			break;
		case PAUSED:
			status = "paused";
			break;
		case CANCELLED:
			status = "cancelled";
			break;
		default:
			status = "finished";
			break;
	}
	info.put<string>("statusInfo",status, string_translator<std::string>());
	return info;
}

void CloudApi::pause_task()
{
	std_msgs::String goal_op;
	goal_op.data = "pause";
	goal_op_pub_.publish(goal_op);
	robot_status_ = PAUSED;
}
void CloudApi::resume_task()
{
	std_msgs::String goal_op;
	goal_op.data = "resume";
	goal_op_pub_.publish(goal_op);
	robot_status_ = RUNNING;
}
void CloudApi::stop_task()
{
	std_msgs::String goal_op;
	goal_op.data = "stop";
	goal_op_pub_.publish(goal_op);
	robot_status_ = CANCELLED;
	ROS_INFO("stop task");
}
void CloudApi::objectCallback(const common_msg::DetectedObjects::ConstPtr& msg)
{
	int size = msg->detected_objects.size();
	bool is_slow = false;
	for(int i = 0;i<size; i++)
	{
		if(msg->detected_objects[i].obj_name == "slow")
		{
			set_speed_shm.shm_write("0");
			is_slow = true;
		}
		if(msg->detected_objects[i].obj_name == "red")
			pause_task();
		if(msg->detected_objects[i].obj_name == "green")
			resume_task();
	}
	if(!is_slow)
		set_speed_shm.shm_write("2");
}